/*
 * This is the code for the Dashboard card in the Auto Rickshaw project
 * 
 * Functionality:
 * Fuel Warning Light		O	PORTB1
 * Battery Warning Light	O	PORTB0
 * Emergency Warning Light	O	PORTB2
 * Indicator Lamp			O	PORTB6
 * Full Beam Lamp			O	PORTB7
 * 
 * Throttle					I	PORTB5(ADC6) INT2 Read this from the ADC obj
 * Break					I	PORTC7 PCINT15 INTERNAL PULL UP
 * 
 * Full Beam Button			I 	PORTC0 PCINT8 INTERNAL PULL UP
 * Left Indicator Button	I	PORTC5 PCINT13 INTERNAL PULL UP
 * Right Indicator Button	I	PORTC4 PCINT12 INTERNAL PULL UP
 * Gears					I	PORTD2 PCINT18 NC
 * Gears					I	PORTD1 PCINT17 NC
 * Emergency Warning Button I	PORTD0 PCINT16 INTERNAL PULL UP
 *
 * @TODO Check functionality on all PINS so they are correctly drawn
 */

#include <tT.h>
#include <env.h>
#include "can.h"
#include "dashboard.h"
#include "adcinput.h"

dashboard_t dashboard = {tt_object(),OFF,OFF,OFF,OFF,OFF,0};

void dashboardCan(dashboard_t *self, can_message_t *msg);

tt_message_t *msg;
//tt_receipt_t msg;

can_listener_t listener_dashboard = {(tt_object_t*)&dashboard, (tt_method_t)dashboardCan, ENV_MSEC(10), 0x400, 0x4f0};

void dashboardInit(dashboard_t *self, void *none){
	/************************************************************************/
	/* Inputs                                                               */
	/************************************************************************/
	//Enabling interrupts on the for PCINT1 and PCINT2
	PCICR	|= (1<<PCIE1);
	PCICR	|= (1<<PCIE2);
	PCMSK1	= 0;
	PCMSK2	= 0;
	
	//Full beam button
	DDRC	&= ~(1<<DDC0);
	PORTC	|= (1<<PINC0);
	PCMSK1	|= (1<<PCINT8);
	
	//Right indicator button
	DDRC	&= ~(1<<DDC4); 
	PORTC	|= (1<<PINC4);
	PCMSK1	|= (1<<PCINT12);
	
	//Left indicator button
	DDRC	&= ~(1<<DDC5);
	PORTC	|= (1<<PINC5);
	PCMSK1	|= (1<<PCINT13);
	
	//Break interrupt
	DDRC	&= ~(1<<DDC7);
	PORTC	|= (1<<PINC7);
	PCMSK1	|= (1<<PCINT15);
	
	//Emergency warning button
	DDRD	&= ~(1<<DDD0);
	PORTD	|= (1<<PIND0);
	PCMSK2	|= (1<<PCINT16);
	
	
	//Gears
	/*DDRD	&= ~(1<<DDD1);
	PORTD	|= (1<<PIND1);
	PCMSK2	|= (1<<PCINT17);
	
	DDRD	&= ~(1<<DDD2);
	PORTD	|= (1<<PIND2);
	PCMSK2	|= (1<<PCINT18);*/
	
	/************************************************************************/
	/* Outputs                                                              */
	/************************************************************************/
	//Battery warning light
	DDRB	|= (1<<DDB0);
	
	//Fuel warning light
	DDRB	|= (1<<DDB1);
	
	//Emergency warning light
	DDRB	|= (1<<DDB2);
	
	//Indicator lamp
	DDRB	|= (1<<DDB6);
	
	//Full beam lamp
	DDRB	|= (1<<DDB7);
	
	can_listener_t * l = &listener_dashboard;
	
	//Enable the ADCS
	ADCinput.ADC6enabled = ON;
	ADCinput.sampleTime = 500;
	
	//Start the ADC sampling
	TT_ASYNC(&ADCinput,ADCRead,TT_ARGS_NONE);
	
	TT_SYNC(&can, canInstall, &l);
	TT_ASYNC(self,toggleIndicatorLamps,TT_ARGS_NONE); //This is somewhat of an ugly hack to remove the context coockie corruption from the warning lights.
	TT_ASYNC(self,throttlePoll,TT_ARGS_NONE);
	ENV_DEBUG("Init of dashboard \n\r");
}

//Some help functions for the can sending
void startWarning(){
	ENV_DEBUG("Starting warning lights \n\r");
	dashboard.warningLights = ON;
	dashboardCanSend(0x706);
	dashboardCanSend(0x726);
}
void stopWarning(){
	ENV_DEBUG("Stop warning lights \n\r");
	dashboard.warningLights = OFF;
	dashboardCanSend(0x707);
	dashboardCanSend(0x727);
}
void startRight(){
	ENV_DEBUG("Start right indicator \n\r");
	//dashboard.rightIndicator = ON;
	dashboardCanSend(0x704);
	dashboardCanSend(0x724);
}
void startLeft(){
	ENV_DEBUG("Start left indicator \n\r");
	//dashboard.leftIndicator = ON;
	dashboardCanSend(0x703);
	dashboardCanSend(0x723);
}
void stopIndicators(){
	ENV_DEBUG("Stop indicators \n\r");
	dashboard.leftIndicator = OFF;
	dashboard.rightIndicator = OFF;
	dashboardCanSend(0x705);
	dashboardCanSend(0x725);
}


//Function to handle the interrupts for PORTC
//@TODO	Make functionality so that when we have an indicator on, and turn on and off warning indicators, it will go back to normal
void buttonFunctionC(void){
	uint8_t savedPin = ~PINC;
	if(!dashboard.bounceC){
		dashboard.bounceC = ON;
		TT_AFTER(ENV_MSEC(20),&dashboard,debounceC,TT_ARGS_NONE);
		if((savedPin & 0x1) == 0x1){
			ENV_DEBUG("Full beam\n\r");
			dashboardCanSend(0x701); //Turn on full beam
			PORTB |= (1<<PINB7);
		
		}else{
			ENV_DEBUG("Full beam, off\n\r");
			dashboardCanSend(0x702); //Turn off full beam
			PORTB &= ~(1<<PINB7);
		}
	
		if((savedPin & 0x30) == 0x30){
			if(!dashboard.warningLights){
				stopIndicators();
				startWarning();
			}			
		}else if((savedPin & 0x20) == 0x20){//Turn on left indicator
			if(!dashboard.leftIndicator && !dashboard.warningLights){
				stopWarning();
				dashboard.leftIndicator = ON;
				//startLeft();
			}
		}else if((savedPin & 0x10) == 0x10){//Turn on right indicator
			if(!dashboard.rightIndicator && !dashboard.warningLights){
				stopWarning();
				dashboard.rightIndicator = ON;
				//startRight();
			}
		}else{//Turn off indicators
			if(dashboard.warningLights){
				stopWarning();
			}else if(dashboard.leftIndicator || dashboard.rightIndicator){
				stopIndicators();
			}else{
				stopWarning();
				stopIndicators();
			}
		}
		if((savedPin & 0x80) == 0x80){
			ENV_DEBUG("Braking\n\r");
			dashboardCanSend(0x721);
		}else{
			ENV_DEBUG("Rollin\n\r");
			dashboardCanSend(0x722);
		}
	}	
}

void debounceC(dashboard_t *self, void *none){
	self->bounceC = OFF;
}

//Function to handle the interrupts for PORTD
void buttonFunctionD(void){
	uint8_t savedPin = ~PIND;
	if((savedPin & 0x1) == 0x1){
		ENV_DEBUG("Button D\n\r");
		if(!dashboard.warningLights){
			dashboard.warningLights = ON;
			dashboardCanSend(0x706);
			dashboardCanSend(0x726);
		}
	}else{
		dashboard.warningLights = OFF;
		dashboardCanSend(0x707);
		dashboardCanSend(0x727);
	}					
}

//Function to send messages to the can-bus
void dashboardCanSend(uint16_t msgId){
	can_message_t msg;
	msg.id = msgId;
	msg.length = 0;
	TT_SYNC(&can, canSend, &msg);
}

void throttlePoll(dashboard_t *self, void *none){
	self->throttle = ADCinput.ADC6val;
	can_message_t msg;
	
	*((uint16_t *) msg.data + 0) = self->throttle;
	
	msg.id = 0x600;
	msg.length = 2;
	
	TT_SYNC(&can, canSend, &msg);
	TT_AFTER(ENV_MSEC(500),self,throttlePoll,TT_ARGS_NONE);
}

void toggleIndicatorLamps(dashboard_t *self, void *none){
	if(self->leftIndicator > OFF && self->warningLights == OFF){
		PORTB ^= (1<<PINB6);
		//Blink left indicators
		if(self->leftIndicator == ON){
			startLeft();
			self->leftIndicator = STARTED;
		}
	}else if(self->rightIndicator > OFF && self->warningLights == OFF){
		PORTB ^= (1<<PINB6);
		//Blink right indicators
		if(self->rightIndicator == ON){
			startRight();
			self->rightIndicator = STARTED;
		}
	}else{
		PORTB &= ~(1<<PINB6);
	}
	if(self->warningLights == ON){
		PORTB ^= (1<<PINB2);
		startWarning();
	}else{
		PORTB &= ~(1<<PINB2);
	}
	TT_AFTER(ENV_MSEC(500),self,toggleIndicatorLamps,TT_ARGS_NONE);
}

void dashboardCan(dashboard_t *self, can_message_t *msg){
	switch (msg->id & 0xF){
		case 0x4:
			PORTB	|= (1<<DDB1);
			break;
		case 0x5:
			PORTB	&= ~(1<<DDB1);
			break;
	}			
}

ENV_INTERRUPT(PCINT1_vect, buttonFunctionC);
ENV_INTERRUPT(PCINT2_vect, buttonFunctionD);
//ENV_INTERRUPT(PCINT1_vect, TT_ASYNC(&dashboard,buttonFunctionCc,TT_ARGS_NONE));
//ENV_INTERRUPT(PCINT2_vect, TT_ASYNC(&dashboard,buttonFunctionDd,TT_ARGS_NONE));